#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# @Time    : 2021/3/21 21:15
# @Author  : Hbber
# @Version : 0.0.0
# @File    : get_data.py
# @Software: PyCharm
# @Org     : AirCas
# @Describe: 全志H3上运行的脚本  用于配置雷达获取数据并通过网络发送出去
__info__ = """

"""

# @Last Modify Time          @Version        @Description
# --------------------       --------        -----------
# 2021/3/21 21:15            0.0.1           None

#
import json
import struct
import sys
import threading
import time
import serial
from queue import Queue
import socket

radarStartEvent = threading.Event()
data_queue = Queue()
userSerial = serial.Serial('/dev/ttyS1', baudrate=115200, timeout=100)
dataSerial = serial.Serial('/dev/ttyS2', baudrate=512000, timeout=100)


def initRadar():
    profile = open('./profile_3_0_0.cfg', mode='r', encoding='utf-8')
    while 1:
        cmdline = profile.readline()
        if cmdline:
            time.sleep(0.2)
            print(cmdline)
            temp = bytes(cmdline, encoding='ASCII')
            userSerial.write(temp)
        else:
            break
    state = 'inited'
    print('Radar state:%s' % state)
    profile.close()


def RadarStart():
    startCmd = bytes('sensorStart\n', encoding='ASCII')
    userSerial.write(startCmd)
    radarStartEvent.set()
    print('Sensor Started')


def RadarStop():
    stopCmd = bytes('sensorStop\n', encoding='ASCII')
    userSerial.write(stopCmd)
    radarStartEvent.clear()
    print('Sensor Stoped')


def read_uart_thread():
    while True:
        data_head = dataSerial.read(1)
        data_head = data_head.hex()
        if data_head == '02':
            data_recv = dataSerial.read(1)
            data_head = data_head + data_recv.hex()
            if data_head == '0201':
                data_recv = dataSerial.read(1)
                data_head = data_head + data_recv.hex()
                if data_head == '020104':
                    data_recv = dataSerial.read(1)
                    data_head = data_head + data_recv.hex()
                    if data_head == '02010403':
                        data_recv = dataSerial.read(1)
                        data_head = data_head + data_recv.hex()
                        if data_head == '0201040306':
                            data_recv = dataSerial.read(1)
                            data_head = data_head + data_recv.hex()
                            if data_head == '020104030605':
                                data_recv = dataSerial.read(1)
                                data_head = data_head + data_recv.hex()
                                if data_head == '02010403060508':
                                    data_recv = dataSerial.read(1)
                                    data_head = data_head + data_recv.hex()
                                    if data_head != '0201040306050807':
                                        continue
                                else:
                                    continue
                            else:
                                continue
                        else:
                            continue
                    else:
                        continue
                else:
                    continue
            else:
                continue
        else:
            continue
        data_package = ''
        end_buff = ''
        while True:
            data_recv = dataSerial.read(1)
            data_recv = data_recv.hex()
            if data_recv == '0f':
                end_buff = end_buff + data_recv
                data_package = data_package + data_recv
                if end_buff == '0f0f0f0f0f0f0f0f':
                    break
            else:
                # not ending
                end_buff = ''
                data_package = data_package + data_recv
        package = bytes.fromhex(data_package)
        # if package attained
        if len(package) == 56:
            temp = package[44:48]
            temp_float = struct.unpack('f', temp)[0]
            data_queue.put(round(temp_float*10000))


def send_data_thread(socket_client, socket_addr):
    print('建立连接： {0}'.format(socket_addr))
    data_queue.queue.clear()
    RadarStart()
    while not getattr(socket_client, '_closed'):
        if data_queue.qsize() > 50:
            data_temp_50 = []
            for i in range(50):
                data_temp_50.append(data_queue.get_nowait())
            data_dict = {
                "timestamp": round(time.time() * 1000),
                "data_50": data_temp_50
            }
            # send
            try:
                socket_client.sendall(json.dumps(data_dict).encode())
            except Exception as e:
                print('失去连接,e:{0}'.format(e))
                break
        else:
            pass
        time.sleep(0.2)  # reduce CPU consumption
    socket_client.close()  # 关闭连接
    RadarStop()
    print('退出线程')


if __name__ == '__main__':
    readSerialThread = threading.Thread(target=read_uart_thread)
    readSerialThread.daemon = 1
    readSerialThread.start()
    initRadar()

    try:
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        # 防止socket server重启后端口被占用（socket.error: [Errno 98] Address already in use）
        s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        s.bind(('0.0.0.0', 6666))
        s.listen(10)
    except socket.error as msg:
        print(msg)
        sys.exit(1)
    print('Waiting connection...')

    while True:
        client, addr = s.accept()  # 建立客户端连接
        # 创建线程
        t_send_data_thread = threading.Thread(target=send_data_thread,
                                              daemon=True,
                                              name=str(addr),
                                              args=(client, addr,))
        t_send_data_thread.start()
